from filter import Adc
from machine import Pin
from machine import Timer
import time
import socket
import network
import gc
class Server(Adc):
  def __init__(self,Pul_pin,Dir_pin,En_pin,Led_pin):
    self.position = 0
    self.speed = 0 
    self.dirction = 0    
    self.sample = 50
    self.a = 2.9423    
    self.pos_list = []
    self.pul_pin = Pul_pin
    self.dir_pin = Dir_pin
    self.en_pin = En_pin
    self.led_pin = Led_pin
    self.adc = Adc()  
    self.wlan = None
    self.socket = None
    self.addr = ('192.168.12.1',10000)
  def blink(self,count,t):
    for i in range(count):
      self.led_pin.value(1)
      time.sleep_ms(int(t/2))
      self.led_pin.value(0)
      time.sleep_ms(int(t/2))
  def wifi_init(self):
    self.wlan=network.WLAN(network.STA_IF)
    self.wlan.active(True)
    self.wlan.disconnect()
    self.wlan.connect('UDPserver','raspberrypi')
    while(self.wlan.ifconfig()[0]=='0.0.0.0'):
      self.led_pin.value(1)
      time.sleep(0.2)
      self.led_pin.value(0)
      time.sleep(0.8)
    self.blink(5,300)
    self.socket=socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    self.socket.setsockopt(socket.SOL_SOCKET,socket.SO_REUSEADDR,1)
    self.socket.setblocking(0)
    self.socket.bind((self.wlan.ifconfig()[0],10000))
  def wifi_receive(self):
    try:
      data,self.addr=self.socket.recvfrom(1024)
      return data
    except:
      return '0'
  def wifi_send(self,info):
    self.socket.sendto(info,self.addr)
  def wifi_disconnect(self):
    self.wlan.disconnect()
  def updata_programe(self):
    p = str(self.pos_list)
    self.wifi_send(p)
  def psiotion_init(self):  
    self.pos_list = []
  def recode_init(self,i):    
    self.adc.init(i)  
  def recode(self,t):    
    self.pos_list.append(self.adc.filter_read())    
    time.sleep_ms(t)    
  def get_power(self):    
    self.en_pin.value(0)
  def cut_power(self):    
    self.en_pin.value(1)  
  def goto(self,position,t):
      t *= 1000
      self.position = self.adc.read_many()    
      if position > self.position:        
        self.change_dir(0)        
        w = int(position-self.position)       
      else:
        self.change_dir(1)        
        w = int(self.position-position)  
      p = int(w*self.a)
      if p != 0: 
        pt = int((t/p)/2)
        for i in range(0,p):          
          self.pul_pin.value(1)          
          time.sleep_us(pt)          
          self.pul_pin.value(0)          
          time.sleep_us(pt)
  def goto_1(self,position):
      self.position = self.adc.read_10()    
      if position > self.position:        
        self.change_dir(0)        
        w = int(position-self.position)       
      else:
        self.change_dir(1)        
        w = int(self.position-position)  
      p = int(w*self.a)
      if p >= 6: 
        pt=int(self.sample*1000/p)
        for i in range(0,p):          
          self.pul_pin.value(1)          
          time.sleep_us(pt)          
          self.pul_pin.value(0)          
          time.sleep_us(pt)
      else:
        time.sleep_ms(self.sample)     

  def run(self):
    self.goto(int(self.pos_list[0]),3000)
    for i in self.pos_list:     
      self.goto_1(int(i))#self.sample
  def get_position(self):
    return(str(self.adc.read_many()))
  def change_dir(self, dir):
    self.dir_pin.value(dir)
  def set_sample(self,s):
    self.sample = s
  def teach(self):
    self.psiotion_init()
    self.recode_init(10)
    while True:
      self.recode(self.sample)  
      info = str(server.wifi_receive())[2:-1].split(',')
      if info[0]=='stop_teach':   
        break
    #print(self.pos_list)
    
gc.collect()
server = Server(Pin(5,Pin.OUT),Pin(4,Pin.OUT),Pin(14,Pin.OUT),Pin(16,Pin.OUT)) 
server.wifi_init()
while True:
  server.blink(1,200)
  info = str(server.wifi_receive())[2:-1].split(',')
  #if info != ['']:
  #  print(info)
  if info[0] == 'get_position':
    server.wifi_send(server.get_position())
  elif info[0] == 'goto':
    server.goto(int(info[1]),int(info[2]))
  elif info[0] == 'set_sample':
    server.set_sample(int(info[1]))
  elif info[0] == 'get_power':
    server.get_power()
  elif info[0] == 'cut_power':
    server.cut_power()
  elif info[0] == 'start_teach':
    server.teach()
  elif info[0] == 'run':
    server.run()
  elif info[0] == 'updata_programe':
    server.updata_programe()
  else:
    pass
